package com.anypi;

import java.util.ArrayList;

import com.tools.Tools;

public class Radian {

	public static byte[] radianToByteArray(byte[] radian) {

		byte tmp = 0;

		ArrayList<Byte> arrayListBit = new ArrayList<Byte>();
		ArrayList<Byte> arrayList = new ArrayList<Byte>();
		ArrayList<Byte> arrayResult = new ArrayList<Byte>();

		for (int i = 0; i < radian.length; i++) {
			arrayList.add(radian[i]);
		}

		// Flush the preambles 1 (0x00)
		while (arrayList.get(0) != (byte) 0xFF) {
			arrayList.remove(0);
		}

		// Flush the preambles 2 (0xFF)
		while (arrayList.get(0) == (byte) 0xFF) {
			arrayList.remove(0);
		}

		for (int i = 0; i < arrayList.size(); i++) {

			for (int j = 0; j < 8; j++) {
				tmp = (byte) ((arrayList.get(i) >> (7 - j) & 0x01));
				arrayListBit.add(tmp);

			}
		}

		while (arrayListBit.size() > 0) {

			if (arrayListBit.get(0) != 1 && arrayListBit.get(1) != 1)
				return Tools.arrayToBytes(arrayResult);

			while (arrayListBit.get(0) != 0) {
				arrayListBit.remove(0);
				if (arrayListBit.size() == 0)
					return Tools.arrayToBytes(arrayResult);
			}

			if (arrayListBit.get(0) != 0)
				return Tools.arrayToBytes(arrayResult);

			arrayListBit.remove(0);
			if (arrayListBit.size() == 0)
				return Tools.arrayToBytes(arrayResult);

			tmp = 0;
			for (int i = 0; i < 8; i++) {
				tmp += (arrayListBit.get(0) << i);
				arrayListBit.remove(0);
				if (arrayListBit.size() == 0)
					return Tools.arrayToBytes(arrayResult);
			}

			arrayResult.add(tmp);
		}

		return Tools.arrayToBytes(arrayResult);
	}

	public static byte[] byteArrayToRadian(byte[] byteArray) {

		ArrayList<Byte> arrayResult = new ArrayList<Byte>();
		ArrayList<Byte> arrayListBit = new ArrayList<Byte>();
		byte tmp = 0;

		//Add first part of preamble
		for (int i = 0; i < 4; i++) {
			arrayResult.add((byte) 0x00);
		}

		//Add second part of preamble
		for (int i = 0; i < 4; i++) {
			arrayResult.add((byte) 0xFF);
		}

		//Transform frame to binary array + add start and stop bits
		for (int i = 0; i < byteArray.length; i++) {

			arrayListBit.add((byte) 0);

			for (int j = 0; j < 8; j++) {

				tmp = (byte) ((byteArray[i] >> j) & 0x01);
				arrayListBit.add(tmp);
			}

			arrayListBit.add((byte) 1);
			arrayListBit.add((byte) 1);

		}

		//Transform bit array to byte array
		while(arrayListBit.size() > 7){
			
			tmp = 0;
			for (int j = 0; j < 8; j++) {
	
				tmp += arrayListBit.get(0) << (7 - j);
				arrayListBit.remove(0);
	
			}
			
			arrayResult.add(tmp);
		}

		tmp = 0;
		for (int j = 0; j < 8; j++) {

			if(arrayListBit.size() > 0){
				tmp += arrayListBit.get(0) << (7 - j);
				arrayListBit.remove(0);
			}
			
			else tmp += 1 <<(7 - j);
		}
		
		arrayResult.add(tmp);
		
		return Tools.arrayToBytes(arrayResult);
	}

}
